% -------------------------------------------------------------------------
% state dynamics
%
% ydot = fy_(t, y , auxdata)
%
% Input argumuents:
% -------------------------------------------------------------------------
% t             [1x1 double]          time                 [ TU ]        
% y             [14x1 double]         state variables      [ - ]
% auxdata       [structure]           parameters           [ - ]
%
% Output argumuents:
% -------------------------------------------------------------------------
% ydot          [14x1 double]         vector field         [ - ]
%
% External functions called:
% -------------------------------------------------------------------------
% None
%
% Copyright(C) 2015/06/22 by Chen Zhang, 
% School of Astronautics, Beihang University
% chenzhang.buaa@gmail.com
% -------------------------------------------------------------------------
function ydot = fy_(t , y , auxdata)
mu = auxdata.mu;
Tmax = auxdata.Tmax;
c = auxdata.c;
p = y(1); ex = y(2); ey = y(3); hx = y(4); hy = y(5); L = y(6); m = y(7);
pp = y(8); pex = y(9); pey = y(10); phx = y(11); phy = y(12); pL = y(13); 
pm = y(14); 
ydot = zeros(14 , 1);

W = 1 + ex * cos(L) + ey * sin(L);
Z = hx * sin(L) - hy * cos(L);
C = 1 + hx^2 + hy^2;

Ckep = sqrt(p / mu) / W;
A = Ckep * [0 ; 0 ; 0 ; 0 ; 0 ; W^3 * mu / p^2];
B = Ckep *  [0 , 2*p , 0;
    W * sin(L) , (W + 1) * cos(L) + ex , -Z * ey;
    -W * cos(L) , (W + 1) * sin(L) + ey, Z * ex;
    0 , 0 , C * cos(L) / 2;
    0 , 0 , C * sin(L) / 2;
    0 , 0 , Z];

% calculate | lambda' * B |
LtB = [pp , pex , pey , phx , phy , pL] * B;
LtB_mag = sqrt(LtB(1)^2 + LtB(2)^2 + LtB(3)^2);

% optimal thrust direction by PMP  
alpha = - LtB / LtB_mag;

% optimal thrust magnitude by PMP
Sw = -Tmax * (LtB_mag / m + pm / c);
if Sw < 0; % Engine turn 'On'
    u = 1;
else % Engine turn 'Off'
    u = 0;
end

% optimal thrust acceleration
ar = u * Tmax / m * alpha(1);
au = u * Tmax / m * alpha(2);
ah = u * Tmax / m * alpha(3);

% state dynamics
A_Bacc = A + B * [ar ; au ; ah];
ydot(1:6) = A_Bacc;
ydot(7) = -u * Tmax / c ;

dW_dex = cos(L);
dW_dey = sin(L);
dW_dL = ey * cos(L) - ex * sin(L);
dZ_dhx = sin(L);
dZ_dhy = -cos(L);
dZ_dL = hx*cos(L) + hy*sin(L);
dC_dhx = 2 * hx;
dC_dhy = 2 * hy;

dCkep_dxx = [1 / (2 * W * sqrt(p * mu)) , -sqrt(p / mu) / W^2 * dW_dex ,...
    -sqrt(p / mu) / W^2 * dW_dey , 0 , 0 , -sqrt(p / mu) / W^2 * dW_dL];

dII_dxx = [2 * au , 0 , 0 , 0 , 0 , 0;
    0 , dW_dex * sin(L) * ar + (dW_dex * cos(L) + 1) * au , ...
    dW_dey * sin(L) * ar + dW_dey * cos(L) * au - Z * ah , ...
    - dZ_dhx * ey * ah , - dZ_dhy * ey * ah , ...
    (dW_dL * sin(L) + W * cos(L)) * ar + (dW_dL * cos(L)...
    - W * sin(L) - sin(L)) * au - dZ_dL * ey * ah;
    0 , -dW_dex * cos(L) * ar + dW_dex * sin(L) * au + Z * ah ,...
    -dW_dey * cos(L) * ar + (dW_dey * sin(L) + 1) * au ,...
    dZ_dhx * ex * ah , dZ_dhy * ex * ah , ...
    (-dW_dL * cos(L) + W * sin(L)) * ar + ...
    (dW_dL * sin(L) + W * cos(L) + cos(L)) * au + dZ_dL * ex * ah;
    0 , 0 , 0 , ah / 2 * cos(L) * dC_dhx , ...
    ah / 2 * cos(L) * dC_dhy , - ah / 2 * C * sin(L);
    0 , 0, 0, ah / 2 * sin(L) * dC_dhx , ...
    ah / 2 * sin(L) * dC_dhy , ah / 2 * C * cos(L);
    -2 * mu * W^3 / p^3 , mu / p^2 * 3 * W^2 * dW_dex , ...
    mu / p^2 * 3 * W^2 * dW_dey , dZ_dhx * ah , ...
    dZ_dhy * ah , mu / p^2 * 3 * W^2 * dW_dL + dZ_dL * ah];

II = A_Bacc / Ckep;
dpp = - [pp , pex , pey , phx , phy , pL] * (Ckep * dII_dxx + II * dCkep_dxx); 
dpm = - u * Tmax * LtB_mag / m^2;

% co-state dynamics
ydot(8:13) = dpp';
ydot(14) = dpm;

end
%--------------------------------------------------------------------------
